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ABSTRACT 


With the Global War on Terrorism (GWOT) taking place in full force, 
autonomous vehicles have become a major asset to government forces. Expansion of 
single vehicle technology to multiple vehicle technology is required in order for the 
United States to stay ahead of its adversaries in the GWOT and other technological fields 
(such as oceanography). Multiple vehicle technology has been explored by many 
different institutions in the recent past (Leonard, 2001 and Kucik, 2003). Expansion of 
this technology will lead to greater autonomy and robustness amongst the vehicles. This 
thesis presents a simulation of a “follow the leader” behavior for multiple Autonomous 
Underwater Vehicles (AUVs). The follower vehicle incorporates the use of forward- 
looking sonar to track the leader vehicle. This process will free up bandwidth between 
acoustic modems; allowing data transfer to occur with greater efficiency. Hydrodynamic 
coefficients are used to develop steering equations that model REMUS through a track of 
specified waypoints similar to a real-world mission track. A two-dimensional forward 
looking sonar model with a 120° horizontal scan and a 110 meter radial range is modeled 
to track the leader vehicle. Resulting bearing and range between the two vehicles is 
incorporated as control for positioning the follower vehicle. 
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I. 


INTRODUCTION 


A. BACKGROUND 

An Autonomous Underwater Vehiele (AUV) is an unmanned, self-propelled 
vehiele that is able to provide data storage as well as make navigational and taetieal 
deeisions based on inputs from onboard sensors. Typieally, data eolleeted from inertial 
sensors is integrated and proeessed by an onboard eomputer in order to estimate the 
position and orientation of the vehiele. This onboard eomputer also eontrols the vehiele; 
allowing it to eomplete its mission with very little human interaetion. AUV’s are 
typieally battery powered. However, many different power sourees have been 
experimented with reeently in order to meet the needs of inereased mission length 
(Miller, 2002). Development of AUV’s began as far baek as 1960. For a history on 
AUV development, see (Blidberg, 2001). AUV’s are developed by many different 
institutions; to inelude government, eommereial, and edueational. 

Today, with the Global War on Terrorism eurrently taking plaee, the United 
States Navy employs a wide range of AUV’s to meet the ever expanding requirements set 
forth by eurrent and future strategy. In reeent years, the United States Navy has adopted 
a strategy that shifts its priorities from blue water to littoral operations and amphibious 
support. Sea Power 21, the eurrent Navy strategy, direets the “use of unmanned 
platforms: Air, Land, Sea, and Undersea for eombat and reeonnaissanee (Clarke, 2002).” 
The Navy has determined that there are nine key mission areas that Unmanned 
Underwater Vehieles (UUV’s) must be designed for in order to be sueeessful in today’s 
battle arena as well as the battle arena 50 years from now. These mission areas inelude: 
1) intelligenee, surveillanee, and reeonnaissanee; 2) mine eountermeasures; 3) anti¬ 
submarine warfare; 4) inspeetion/identifieation; 5) oeeanography; 6) 
eommunieation/navigation network node; 7) payload delivery; 8) information operations; 
and 9) time eritieal strikes (UUV Master Plan, 2004). 
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B. MOTIVATION 

The primary motivating factor behind this research is to allow formation flying of 
AUV’s to occur without the use of an acoustic modem as the primary source of 
communicating position between the vehicles. This will free up bandwidth exchanged 
between acoustic modems located on each vehicle. This additional bandwidth will allow 
for an increased efficiency in the transfer of mission data between vehicles; which in turn 
will allow for operators to receive data quicker (helping with planning and executing of 
real world missions). 

A secondary motivating factor behind this research is to give vehicles the 
capability to become more robust and autonomous. The addition of forward looking 
sonar to the Remote Environmental Measuring Unit System (REMUS) vehicle will allow 
this to occur. In addition to multiple vehicle control, forward looking sonar will allow for 
obstacle avoidance (Eodrea, 2002). This thesis will present a solution to the formation 
flying problem for multiple AUV’s. 


C. FORMATION FLYING FOR AUTONOMOUS UNDERWATER 
VEHICLES 

In order to stay ahead of its adversaries, the United States must switch from single 
AUV technology to multi-AUV technology. This switch in design and mission planning 
has been examined by many different institutions in recent years. Early research in this 
area had vehicles relying on beacons or locator sensors in order to control the formation. 
More recently, “follow the leader” behaviors have been designed to rely on ranging and 
intention information shared between vehicles using acoustic communications (Kucik, 
2003). This thesis will go one step further and concentrate on using a “follow the leader” 
behavior that relies on ranging and bearing information received from forward looking 
sonar. 

D. THE REMUS VEHICLE 

The original REMUS vehicle was developed at Wood’s Hole Oceanographic 
Institute (WHOI). In 2001, REMUS entered commercial production and is now sold by 
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Hydroid, Inc. REMUS is used for a number of applications including harbor security and 
mine countermeasure operations. 

It is designed to perform in the Very Shallow Water (VSW) zone from 40 to 100 
feet. As seen in Figure 1, it is a two-man portable system that is 7.5” (19 cm) in 
diameter, 63” (160 cm) long, and weighs 80 pounds (37 kg). REMUS can reach a 
maximum speed of 5.45 knots (2.8 m/s) and a maximum depth 328 feet (100 m). Table 1 
includes functional and physical specifications of the REMUS vehicle. 



Figure 1. REMUS Vehicle (from Hydroid Inc., 2005) 


REMUS can be configured with many different types of sensors, such as: side 
scan sonar, an acoustic doppler current profiler (ADCP), inertial navigation system, and 
acoustic modem. The navigation system includes a compass, the above-mentioned 
ADCP to provide speed over ground when ground lock is available, and an acoustic long- 
baseline (LBL) system to correct accumulated dead reckoning errors. Currently side-scan 
sonars are used to detect object on or near the sea floor. However, the addition of 
forward-looking sonar would give it the ability to detect objects (such as another AUV or 
uncharted sea mounts) in front of the vehicle. 
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PHYSICAL/FUNCTIONAL AREA 

CHARACTERISTIC 

Vehicle Diameter 

19 cm 

Vehicle Length 

160 cm 

Weight in Air 

37 kg (<80 lbs.) 

Trim Weight in Air 

1 kg 

Maximum Operating Depth 

100 meters 

Energy 

Ikw-hr internally rechargeable 

Lithium ion 

Endurance 

22 hours at optimum speed of 

L5m/s (3 knots). 8 hours at 

2.5m/s (5 knots) 

Propulsion 

Direct dive DC brushless motor 
to open three bladed propeller 

Velocity Range 

0.25 to 2.8 m/s variable over 
range 

Control 

2 coupled yaw and pitch fins 

On/Off 

Magnetic switch 

External Hook-up 

Two pin combined Ethernet, 
vehicle power and battery 
charging; 4pin serial connector 

Navigation 

Long base line; Ultra short 
base line; Doppler assisted 
dead reckon; (Optional: GPS) 

Transponders 

20-30 kHz operating frequency 
range 

Tracking 

Emergency transponder, 
mission abort, and ORE 

Trackpoint compatible 

Sensors Doppler Velocity Log 

RDl 1.2 MHz up/down looking 

Side Scan Sonar 

600 or 900 kHz MSTL AUV 
model 


Table 1. REMUS Specifications (from Hydroid Inc., 2005) 

E. THESIS STRUCTURE 

The intent of this research is to develop a forward looking sonar model that 
supports “follow the leader” behaviors of multiple REMUS vehicles. This is a two step 
process accomplished through the following; firstly, develop a robust steering model of 
two independently controlled REMUS vehicles and have them navigate a set of 
waypoints; secondly, adapt the steering model so that the second vehicle steering is 
dependent and controlled by the first vehicle’s steering model. 
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Chapter II will focus on the development of the equations of motion for the 
REMUS AUV. Chapter III explains the theory and equations used for triangulating 
positions using ranging information. 

Chapter IV describes the steering control laws associated with the EOM for REMUS. 
Chapter V will present a simulation for the “follow the leader” behavior using forward 
looking sonar. Chapter VI provides thesis conclusions and recommendations for future 
work. 
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II. STEERING MODEL 


A. GENERAL 

Rigid body models are formed in order to analyze, predict, and control motion 
behavior of autonomous machines that travel over land, air, and undersea. Each type of 
vehicle model differs in only the terms of the forces applied to produce motion. 
However, these forces are often controllable and can thus be studied from a prospective 
of stabilization. This chapter will only deal with the modeling of underwater vehicles. 
The approach taken with underwater vehicles is that of a moving body in free space 
without constraint. The forces applied to underwater vehicles include the following: 
inertial, gravitational, hydrostatic, propulsion, thruster, and hydrodynamic lift and drag 
forces. (Healey class notes). 

B. EQUATIONS OF MOTION IN THE HORIZONTAL PLANE 

The following paragraphs describe a simplified development of the steering 
model used to control the REMUS vehicle. Eor a more detailed development, see 
(Healey, 1995). This model was adapted from that of the ARIES AUV (Healey and 
Marco, 2001) and is based on the following assumptions: 

• the vehicle behaves as a rigid body 

• the earth’s rotation is negligible for acceleration components of the 
vehicle’s center of mass 

• the primary forces that act on the vehicle are inertial and gravitational in 
origin and are derived from hydrostatic, propulsion, thruster, and 
hydrodynamic lift and drag forces. 

Before describing the equations of motion (EOM) that govern the REMUS 
steering model, a coordinate system for the vehicle and its surrounding area must be 
defined. The EOM are derived using a Newton-Euler approach that relates the position 
and motions in the local plane to those in the global plane. The geometry of the local and 
global coordinate system can be seen in Eigure 2. 
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▼ z 

Figure 2. Local and Global Coordinate System (from Marco and Healey, 2001) 


In order to convert from a local velocity vector[M,v, w], where u is surge, v is 

sway, and w is heave, to a global velocity vector[x,F,z], a transformation matrix 

containing ‘Euler’ angles {(j),6,ij/) must be defined. The transformation matrix (T) is 
defined as follows: 




cos y/ cos 6, 

cos y/ sin ^ sin ^ - sin^//cos (/), 
cos y/sm 6 cos (/) + sin^z/'sin (j). 


sin y/cos 0, - sin 6 

sin sin ^ sin ^ + cos y/ cos (j), cos ^ sin ^ 

sin sin ^ cos ^ - cos y/ sin (j), cos ^ cos ^ 


( 1 ) 


Transformation from a global velocity vector to the local velocity vector occurs as 
follows: 


u 


~X~ 

V 

w 

= T{(^,0,y/)» 

Y 

Z 


( 2 ) 


Transformation from a local velocity vector to a global velocity vector occurs as follows: 


~x^ 


u 

Y 

Z 

= T-\^,0,y/)* 

V 

w 


( 3 ) 
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The global angular velocity vector [ p,q, r] can be transformed into the rates of change of 
the ‘Euler’ angles as follows: 

^ 1 sin ^ tan 0 cos ^ tan ^ p 

0=0 cos^ -sin^ q (4) 

xj/ 0 sin^/cos0 cos cos ^ r 

Healey (1995) derives the equations of motion for a six degree model as: 

SURGE EQUATION OE MOTION 

m Ur^vj + w^q-X(.{^q^ + r^^ + y(.[pq-r) + ZQ{pr + q) +[W-B)sin0 = X^ (5) 

SWAY EQUATION OE MOTION 

m v^+u^r-w^p + X(.[pq + r)- y^i^p^ + + Z(j{qr-p) -(iy-fi)cos0sm^ = T^ (6) 

HEAVE EQUATION OE MOTION 

m w^-u^q + v^p + X(j[pr-q) + y^[qr + p)-Z(;{^p^ +q^y^ + {W-B)cos0cos^ = Z^ (7) 

ROEE EQUATION OE MOTION 

hP + {lz-^y)^^ + ^xy{pr-g)-Iy,(q^-r")~I,,{pq + r) + m[y^{w-u^q + v^p) ( 8 ) 

~^G {Vr+^r^~'^rP)^~{yG^ + (9) 

PITCH EQUATION OE MOTION 

Iyq + {lz-^z)p^-^xy{qr + p) + Iy,{pq-r) + I^^(p^-r^)-m[x^{w-u^q + v^p) (10) 

-Zg {ur ~ + i^G^ ~ XgB)cos0 COS (p + ( ZfjW - ZgB)sin0 = 

YAW EQUATION OE MOTION 

h^^[h~^x)pq-lxy[p^ -q^)-lyz{pr^q)^hz{qr-p)^f^[_^G{Vr+U/-W^p) (II) 
-y^ (m^ - v^r + w^q)^ -{x^W - x^B ) cos^sin^- ( y^W - y^B )sin^ = A^ 
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Where; 

W = weight 
B = buoyancy 

I = mass moment of inertia terms 

Ur, Vr, Wr = componcnt velocities for a body fixed system with respect to the water 
p,q, r= component angular velocities for a body fixed system 
xb, ys, Zb = position difference between geometric center and center of buoyancy 
xg, yc, Zg = position difference between geometric center and center of gravity 
Xfi Yfi Zfi Kf, Mfi Nf = sums of all external forces acting in the particular body fixed 
direction 

In addition, he presents a simplified version of these equations of motion. In order to 
simplify the initial equations of motions the following assumptions were made; 

• the center of mass of the vehicle lies below the origin 

• Xg and yg are zero 

• the vehicle is symmetric in its inertial properties 

• motions in the vertical plane are negligible (i.e., [Wr, p, q, r, Z, (p, 0 ]=0) 

• Ur equals the forward speed, Uo 
The simplified equations of motion are thus; 


Ur=U„ 

(12) 

mv^ = ~mU j + XYj (t) 

(13) 

II 

(14) 

\j/ = r 

(15) 

X =U^cosip-v^ smy/+U^^ 

(16) 

Y - U^ smy/-v^ cosy/+U^ 

(17) 


C. HYDRODYNAMIC COEFFICIENTS 

Healey proposes that due to symmetry of the vehicle, one can heuristically 
determine that only a subset of motions would affect the loading in any particular 
direction (Healey class notes) and uses the following expressions to describe 
hydrodynamic forces of sway and yaw; 
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AF^ = f{v^,dv^ / dt,r,dr/dt,p,dp/dt,t) 


( 18 ) 


AA^/ = f{p,dp ! dt,v^,dvj dt,r,dr ! dtp) (19) 

Sway, yaw, and roll motions are coupled. However, roll motion is often only coupled 
one way and not considered when evaluating horizontal plane steering. The 
hydrodynamic forces for sway and yaw are linearized using Taylor series expansion to 
determine ‘hydrodynamic coefficients.’ The coefficients are dependent on the shape 
characteristics of the vehicle and have significant affect on the natural stability of the 
vehicle. The expression for the transverse (sway) force is: 

Yf=Y,v^ + Y^v^ + Y,r + Y^r ( 20 ) 

and the expression for rotational (yaw) force is: 

Nf = + N^v^ + N,r + N^r ( 21 ) 


This leads to: 


and 


dY. dNf dY, dYf 

y __Y =_T- Y =_£_■ Y =_ 

^Vr -) 

(7V„ dr ov, dr 


dNf dNf dNf dNf 

N.. =^; N =-^; N, =^;N,- 


dv. 


dr dv^ 


dr ’ 


Where: 

Y. = coefficient for added mass in sway 
Y. = coefficient for added mass in yaw 
Y^ = coefficient of sway force induced by side slip 
Y^ = coefficient of sway force induced by yaw 


( 22 ) 


(23) 
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N- = coefficient for added mass moment of inertia in sway 
N- = eoeffieient for added mass moment of inertia in yaw 
= eoeffieient of sway moment from side slip 
= eoeffieient of sway moment from yaw 

The hydrodynamic coefficients for steering for the REMUS vehicle were adapted 
from thesis work performed by MIT (Prestero, 2001) establishing estimates of all vehicle 
coefficients. Upon re-caleulation, Fodrea (2002) adjusted the hydrodynamic coefficients 
to aeeount for variation in experimental data. Table 2 lists the REMUS hydrodynamie 
coefficients for the steering model used during this experiment. 



-3.55e01 kg 

B 

1.93 kg rr./rad 

B 

-6.66e01 kg/s (Same as Zw) 

Yr 

2.2 kg rr./s (Same as Zq) 

N 

'V 

1.93 kg m. 


-4.88 kg m.'/rad 


-4.47 kg m./s 

Nr 

-6.87 kg m."/s (Sam.e as Mq) 

N, 

-3.46e01/3.5 kg m:/s^ 

Y, 

5.06e01/3.5 kg m./s" 


Table 2. REMUS Hydrodynamie Coefficients for Steering (from Fodrea, 2002) 
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The dynamics of the vehicles are defined as; 


mv^ =-mr + Y.y^+Y^v^ + Y.r + Y/ + YgdXt) (25) 

1J = N.y^ + N^y^ + Ny + Ny + NgS^t) (26) 

\j/=r (27) 


D. VEHICLE KINEMATICS 

The kinematics of the vehicle is described by Equations (25) and (26). Ucx and 
Ucy are the current velocities in the associated direction. The kinematic equations, along 
with the heading rate, compose the steering dynamics of REMUS and can be expressed 
as follows; 


1 

s 

1 


o' 




Y^-mU, 

O' 



1 

1__ 



0 

r 

— 



0 

r 

+ 


0 

0 

1 



0 

0 

1 



0 


(28) 


where d^t) represents the control input for both rudders. 


E. VEHICLE DYNAMICS 

The final assumption made for vehicle dynamics (Johnson, 2001) is that the cross 
coupling terms in the mass matrix is zero. Thus, the final vehicle dynamics are defined 
as; 


1 

3 

1 

0 

0' 




Yr-mU, 

0' 



1 

1__ 

0 


0 

r 

= 



0 

r 

+ 

N, 

0 

0 

1 



0 

0 

1 



0 




(29) 
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III. TRIANGULATION THEORY: GEOMETRY AND EQUATIONS 

A. INTRODUCTION 

Triangulation is one of the most eornmon methods for determining position. It is 
used on many different navigation systems including; Long Base Line (LBL), Global 
Positioning System (GPS), and LORAN. The simplest method of triangulation is where 
the vehicle’s range from two known points is used to determine the vehicle’s position. 
However, despite its simplicity, there are still some ambiguities associated with this 
process. Figure 3 shows how an LBL system is not able to determine where the vehicle 
is located because the range rings centered on the transponders intersect in two places. 



Figure 3. LBL System (from Kucik, 2003) 

Throughout this thesis, it was assumed that the leader vehicle had an advanced 
navigation system (to include GPS) onboard and can accurately determine its position 
throughout the simulation. 
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B. TRIANGULATION GEOMETRY AND EQUATIONS 

Triangulation geometry can be used to determine the range and bearing between 
the leader and follower vehicles based on their position. Figure 4 is a simplified 
triangulation computation using the x and y values of both vehicle’s position. 



As seen in Figure 4, and based on the Law of Cosines, the following formulas are defined 
for range and bearing; 

rangc = p-Y^f + {X-X^f (30) 

where 

Y = the y-position of the leader vehicle 
Y^ = the y-position of the follower vehicle 

X = the x-position of the leader vehicle 
Xj- = the x-position of the follower vehicle 

and 

bearing=atan2((y -Y^),{X - X f (31) 

where y/^- = follower vehicle heading 
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IV. CONTROL METHODS AND ARCHITECTURE 


A. GENERAL CONTROL THEORY 

An underwater vehicle operates with six degrees of freedom and must respond to 
hydrostatic and hydrodynamic forces from an ever changing ocean environment. In 
addition to responding to environmental factors, underwater vehicles also must overcome 
the fact that actuator dynamics are small, power and control is limited to onboard 
capacity of the vehicle, and human intervention to correct faults is not possible during a 
mission (Fodrea, 2002). In spite of the above conditions, feedback control has been a 
suitable solution used to provide commands to actuators that control and stabilize the 
motion of underwater vehicles (Healey and Marco, 2001). 

One of the main objectives of autonomous vehicles is that they must be robust. 
Robustness is obtained by using feedback of key motion variables as obtained by sensors 
that drive actuators (Healey class notes). This in turn acts an autopilot and maneuvers the 
vehicle as set during mission pre-planning. Marco describes four different autopilots for 
flight maneuvering control. These consist of independent diving, steering/heading, 
altitude above bottom, and cross-track error controllers. The four modes are decoupled 
and are based on sliding mode control (SMC) theory. Sliding mode controls can 
compensate for nonlinear systems (Healey, 1992). It is a robust method with a theory 
which allows n* order systems to be effectively replaced by a (n-I) order system. Two 
tuning factors are used in this model to include Eta_FlightHeading, , and 
Phi_FlightHeading, (j ), as seen in Appendix A. 

B. REMUS CONTROL ARCHITECTURE 

The original REMUS steering model, used in this thesis for the leader vehicle, 
was developed by Fodrea (2002). Both the leader and follower vehicles are modeled in 
two dimensional space. Due to this fact, the steering controller is the only autopilot 
controller necessary for modeling the leader vehicle. The leader vehicle is a second order 
model that uses r and Xj/ as feedback. 
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In order to reach all of the desired waypoints, line-of-sight guidance is also implemented. 
The follower vehicle’s steering controller is not as advanced as the leader vehicle. The 
follower vehicles position is maintained by control laws incorporating range and bearing 
to the leader vehicle that is received from forward looking sonar. 


C. SLIDING MODE CONTROL 

A multivariable sliding mode controller is used to provide an accurate steering 
model. A multivariable controller is used with predominantly linear systems as opposed 
to the SMC methods used for nonlinear systems (Healey, 1992). To create the SMC, the 
general form of the equations of motion is used; 

x-Ax + Bu (32) 


wherexe 91"*', Ag , and u is the rudder angle. The goal of a 

sliding surface it do drive the state to a stable solution ((7 = 0, (7 g M^*'). The sliding 
surface is defined as; 


cj = s'r, x = x-x^^^ (33) 

where s’ is a vector of directions in the state error space. The elements of (J are the 
lengths of the projections of the state error vector, x^^^ is a variable created as a 
command signal to track, x is the state error which is required to be driven to zero so the 
command state equals the actual state. By definition of the sliding mode controller, the 
system dynamics must exhibit stable sliding on the surface when a = 0. Therefore, s’ can 
be determined by observing that the closed loop dynamics are given by the poles of the 
closed loop matrix as, 

(A - hkj ) = A^, with kj = [^' 5] ' 5' A (34) 

where k 2 is chosen by pole placement and AcS’ = 0 to achieve the condition a = 0. The 
eigenvectors of the Ac matrix determine the linear state feedback gains for each state to 
define the sliding surface as follows; 
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= ^2 - r{t)) + (0 


(35) 


The poles selected for the REMUS model of the leader vehicle were similar to those 
selected by Fodrea (2002) in here simulation. As seen in Appendix A, the poles are 
places at [-1.4 -1.45 0.0]. A pole must be placed at the origin in order to represent the 
single sliding constraint for the single input system. The remaining poles are in the open 
left hand plane; which is a requirement for stable dynamics. The gains obtained from this 
pole placement, using the MATLAB command “place”, where [kl k2 k3] = [0.769 -0.6 
0.0] for [v,r,y/^ respectively. Using the gains determined from pole placement and the 
sliding surface defined in Equation(34), the commanded rudder in the EOS controller for 
the leader vehicle becomes; 


dr{t) = -k2 * r(t) *7]* tanh 


V ^ y 


where rj and (j) are tuning factors equal to 0.5 and 0.1. 


(36) 


D. LINE OF SIGHT GUIDANCE 

A Fine of Sight (EOS) controller is used on the leader vehicle. The purpose 
behind its use it reduce the heading error to zero. Once again, this controller was 
developed for the REMUS vehicle by Fodrea (2002). It uses a follow-the-rabbit 
technique which is similar in nature to the transducer based dead-reckoning approach 
with which REMUS operates with in the real world. This controller directs the vehicle 
towards the current waypoint. It achieves this by determining the heading error, 
which is defined as: 

LOS rack -V^iO (37) 

where 

¥it),rack = arctan(y(0„^,(,), i(Ov.pr(o) (38) 

Figure 3 is a graphical representation of how the REMUS vehicle model incorporates an 
additional dead reckoning on the track towards the next waypoint. The distance to this 

point is incorporated into the heading error as follows: 
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¥it):^OS = Vii) track 


y/(t) — arctan 


^ cte{t) ^ 
V rabbit y 


where rabbit is the look ahead point and ete is the eross traek error. 


(39) 



Figure 4. Traek Geometry and Velocity Vector Diagram (from Fodrea, 2002) 

As the leader vehicle approaches the waypoint, it must determine when to turn. In 
order to do this, REMUS will begin tracking the next waypoint when approaching the 
present waypoint based on the following formula: 

sqrt{X _ Way _ Error{tY + F _ Way _ Error{tY <= VF _ 7? |5(t) < 0.0| ss{t) < rabbit (40) 

where W_R is the watch radius around the waypoint, s is the distance remaining on the 
track, and ss is the radial distance to go to the next waypoint. 
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E. CONTROL LAWS FOR FOLLOWER VEHICLE 


One of the primary objeetives of this thesis was to design a set of eontrol laws that 
would position the follower vehiele. As seen above with the leader vehiele, the eontrol 
laws for the follower vehiele were designed based on sliding mode eontrol. Control laws 
for the follower vehiele were based on ineorporating range and bearing to the leader 
vehiele that was reeeived from forward looking sonar. 

Based on the range between the leader and the follower vehiele, the follower 
vehiele would adjust its speed (U) in order to aehieve the eommand range (Rcom). Range 
error is defined as: 

R = (R(0-R_) (41) 

In order to determine the speed of the follower vehiele, the following formulas are 
defined: 

=-^sgn(cr«) (42) 

where <7^,= R . Also, 


(j,=R = {R-0) 


R is defined as: 


R = 


R 


{K-x) + 


R 


(y.-y) 


where 

= leader vehiele’s x-position 
X - follower vehiele’s x-position 
cosiy/J 
x = U cos{y/) 

= leader vehiele’s y-position 


(43) 


(44) 
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y = follower vehicle’s y-position 


yo=U, sin((/J 
y = U sin(^^) 

Combining Equations (42) and (44) and solving for U results in the following formula: 

jj ^ -sgn(^)i? + .fo(^-^o)+y„(y-yJ 
cos ?^(x - jTq ) + sin ?^( y - y J 

Based on the bearing between the leader and the follower vehicle, the follower 
vehicle would adjust its heading (y^f) in order to achieve the command bearing 

Bearing error is defined as: 

( 46 ) 

In order to determine the heading of the follower vehicle the following formulas are 
defined: 

=-^sgn(y5 + /ly^) (47) 

= -rj sgn(y9 + Ar) (48) 

After a trial and error period, the following formula was produced in order to provide 
stable results. 

<J^=-5*^ + 10*r (49) 

The formulas listed throughout this chapter are incorporated into the model as 
seen in Appendix A. Results based on these formulas are discussed later in this thesis 
(Chapter V). 


22 



V. VEHICLE SIMULATION 


A. INTRODUCTION 

A MATLAB script file (Appendix A) was written in order to simulate multi- 
vehiele operations. This script file contains control laws and cooperative behaviors that 
support and evaluate the “follow the leader” behavior presented in Chapter IV. The 
following settings were incorporated into the simulation: 

• The minimum distance between the leader vehiele and the follower 
vehiele (Reom) was set to 10 meters. 

• The leader’s vehicle speed is set to 1.543 m/s. 

• The follower’s minimum and maximum speed is set to 1 m/s and 2 m/s, 
respeetively. 

B. REMUS SEARCH PATH 

Due to the autonomy of the vehicle, REMUS is pre-programmed with a planned 
path of travel before the mission begins. The seareh path is commonly referred to as the 
lawnmower teehnique and is used to cover a square grid area. This thesis models a 
REMUS path the uses rows approximately 150 meters in length with 100 meters of 
separation as seen in Eigure 5. 
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Eigure 5. Typical REMUS Search Path 
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C. SONAR MODEL 

This model uses a two dimensional forward looking sonar with a 120° horizontal 
sean and a 110 meter radial range as seen in Figure 6. This is an estimated range based 
on a viable 400KHz sonar frequency. The use of forward looking sonar would alleviate 
exchanging position information via acoustic modem. Resulting bearing and range is 
used to control position of the follower vehicle. 



Figure 6. Forward Looking Sonar Model (from Fodrea, 2002) 

D. MULTIPLE VEHICLE SIMULATION (NO CURRENT) 

The first objective of this simulation was to have the leader and follower vehicle 
navigate a set of 7 waypoints depicting a typical “real world” search path. At this point, 
current was not injected into the simulation model. Figure 7 depicts the tracks for the 
leader and follower vehicles during the mission simulation. 
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Figure 8 shows an initial offset of the follower vehicle. This offset was 
performed in order to show the initial rendezvous between the leader and follower 
vehicles. 
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Figure 8. Initial Rendezvous Between Vehicles 


Figures 9 and 10 show the range and bearing between the leader and the follower 
vehicle. Range and Bearing were the main variables in controlling the position of the 
follower vehicle. The command range was set to 10 meters and the command bearing 
was set to 0 degrees for this simulation. 

From Figure 9, it is seen that initial range is approximately 35 meters. This is due 
to the initial offset. The range then decreases down to 10 meters as expected. Also, the 
range drops below 10 meters at 5 different locations during the simulation. This drop in 
range corresponds to the leader vehicle turning to the next waypoint. 
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Figure 9. Range Between Leader and Follower Vehicle 


From Figure 10, it is seen that initial bearing is approximately -45 degrees. As 
stated above, this is due to the initial offset of the follower vehicle. The bearing then 
decreases down to 0 degrees as expected. Bearing between the two vehicles changes 
during the turns as did the range 



Figure 10. Bearing Between Leader and Follower Vehicle 
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Figures 11 and 12 contain a “zoomed in” look at the mission simulation. 
Specifically, Figure 11 is a “zoomed in” look at the vehicles while they are traveling in a 
straight line. Figure 12 is a “zoomed in” look at the vehicles during a turn. 


Multi-Vehicle Mission Run 



Figure 11. Zoomed In Look (Straight Path) 
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Figure 12. Zoomed In Look (Turn) 
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E. MULTIPLE VEHICLE SIMULATION (WITH CURRENT) 

The next step of this thesis was to add the affects of current in to the simulation. 
Current was added to achieve a more accurate simulation of a real world mission. Figure 
13 depicts the tracks for the leader and follower vehicles during the mission simulation 
containing the affects of current. Looking at Figure 9, the follower vehicle does not 
appear to be directly behind the leader vehicle as seen in the earlier simulation. 


Multi-Vehicle Mission Run 



Figure 13. Leader and Follower Vehicle Path (affected by currents) 


Figures 14 and 15 contain a “zoomed in” look at the mission simulation. 
Specifically, Figure 14 is a “zoomed in” look at the vehicles while they are traveling in a 
straight line. Figure 15 is a “zoomed in” look at the vehicles during a turn. 

From Figure 13, it initially looks like the follower vehicle is offset in comparison 
to the leader vehicle. However, the leader vehicle actually has a range of 10 meters and a 
bearing of 0 degrees. This is shown in Figures 16 and 17. Both the leader and follower 
vehicles are “crabbed” in order to offset the current and reach the desired waypoints. 
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Figure 14. Zoomed In Look (Straight Path w/ Currents) 


Multi-Vehicle Mission Run 



Figure 15. Zoomed In Look (Turn w/ Currents) 
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Figure 16. Range Between Leader and Follower Vehiele (w/ currents) 
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Figure 17. Bearing Between Leader and Follower Vehicle (w/ currents) 
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VI. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 

This thesis modeled and simulated formation flying of multiple AUV’s using 
forward looking sonar as a method of eontrolling the position of the follower vehiele in 
the formation. The researeh eompleted by this thesis provides the tools neeessary to 
develop more sophistieated eooperative behaviors (i.e., multiple vehiele obstaele 
avoidanee). The proposed eontrol laws presented in this thesis sueeessfully operates in 
the simulation environment. The follower vehiele adjusted its speed and heading 
aeeordingly to obtain the eommanded position aeeurately. 

B. RECOMMENDATIONS 

There are many areas in whieh this thesis work ean be improved upon to build a 
more eomplete and robust formation model for the REMUS vehiele. One sueh area is the 
development of a three dimensional model. This would require the addition of depth to 
the sonar sean. This in turn would require the modifieation of the vehiele equations of 
motion to inelude diving and elimbing maneuvers. 

Another area of future work is to implement obstaele avoidanee algorithms into 
the simulation. Obstaele avoidanee algorithms have been designed for use with REMUS 
vehieles using forward looking sonar. However, it has yet to be expanded to multiple 
vehiele formations. 

Easily, upon eompletion of simulation modeling, real world experimentation must 
take plaee in order to validate results reeeived form various simulations using forward 
looking sonar. After initial experimental data has been eolleeted, the simulation model 
may need to be refined to aeeount for environmental faetors not originally foreseen by the 
model. 
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APPENDIX 


% Multi - Vehicle (Dependent Control) 

% This m-file simulates a multi-vehicle follow the leader formation. 

% It uses corrected hydrodynamic coeff from MIT to develop a steering 
% model for the leader vehicle. The follower vehicle is positioned 
% based on bearing and range to the leader vehicle received from a 
% forward looking sonar that is attached to the follower vehicle. 

clear,close all; 
clc 


DegRad = pi/180; 
RadDeg = 180/pi; 


% REMUS Characteristic 
L = 1.33; % 

W = 2.99e02 ; % 

g = 9.81; % 

m = W/g; % 

V = 1.543; % 

rho = 1.03e03; % 

D = .191; % 


Specifications 
Length in m 
Weight in N 

Acceleration of gravity in m/s 

Mass in kg 

Max Speed in m/s 

Density of Salt H20 in kg/m^3 

Max diameter in m 


^2 


% State Model Parameters 

Uo = 1.543; % in m/s 

Boy = 2.99e02; 

xg = 0; yg = 0; zg = 1.96e-02; % in m 


ly = 3.45; % kg/m^3 (from MIT thesis) 

Iz=Iy; 


% MIT REMUS Coeff (Dimensionalized) 

Nvdot = 1.93; 

Nrdot = -4.88; 

Yvdot = -3.55e01; 

Yrdot = 1.93; 

% Nv = -4.47; should be same as Mw which is stated as +30.7 
% should be -9.3 but going by Hoerner eqn, we get about 4.47 
Nv = -4.47; 

Nr = -6.87; % Same as Mq; 

Yv = -6.66e01; 

% Same as Zw; Note should be -6.66el from MIT thesis not 2.86e01 
Yr = 2.2 ; % Same as Zq = 2.2; MIT has miscalculation 

Nd = -3.46e01/3.5; % Nd and Yd scaled by 3.5 to align w/exp data 

Yd = 5.06e0l/3.5; 

% REMUS Steering Equations 

MM=[(m-Yvdot) -Yrdot 0;-Nvdot (Iz-Nrdot) 0;0 0 1]; 

AA=[Yv (Yr-m*V) 0;Nv Nr 0; 010]; 

BB= [Yd;Nd;0] ; 

A=inv(MM)*AA; B=inv(MM)*BB; C=[0,0,1]; D=0; 
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A2=[A(1:2,1),A(1:2,2)];B2=[B(l);B(2)]; 
xss=inv(A2)*B2; 
poles = eig(A2); 

RadGy = sqrt(Iz/(W/g)); 

RadCurv = Uo/(xss(1)); 

sideslip = atan2(xss(1),Uo)*180/pi; 


% in m 
% in m 
% in deg/s 


[num,den]=ss2tf(A,B,C,D); z=roots(num); p=roots(den); 


% Desired closed loop poles for sliding 
k=place(A,B, [-1.4, -1.45,0.0]); 


% Closed loop dynamics matrix 
Ac=A-B*k; 

[m, n] =eig (Ac ' ) 

S=m(:,3); 


%Define Sonar Grid Parameters: 
%sonrange = 110; 

%theta = 2*pi/3; 

%dtheta = l*pi/180; 

%thetab = theta/dtheta; 

%offset = 0; 


% radial range in m based on FLS chart 
% angular arc in rad 
% angular bin length in rad 
% number of bins angularly 


% Set time of run 
dt = 0.125/2; 
t = [0:dt:1800] ' ; 

size (t) ; 

% Set initial conditions 
start=10; 

V(1) = 0.0; 

r(1) = 0.0; 

rRM (1) = r (1) ; 

r_com = zeros(1,length(t)-1); 

vf(1) = 0.0; 

rf(1) = 0.0; 

rRMf (1) = rf(1) ; 

r_comf = zeros(1,length(t)-1); 

% This is the initial heading of the vehicle 
psi(l) = 0.0*DegRad; 

psif(1) = 0.0*DegRad; 

% This is the initial position of the vehicle 
X(1) = 0.0; % in m 
Y(l) = 100.0; 

Xf(1) = -25.0; % in m 

Yf(l) = 125.0; 
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% This data from track.out file 
No_tracks=7; 


[100.0 

100.0 

2.75 

2.75 

0 

1.25 

1.00 

0 

25.00 

8.00 

40.00 

200.0 

100.0 

2.75 

2.75 

0 

1.25 

1.00 

0 

25.00 

8.00 

200.00 

200.0 

250.0 

2.75 

2.75 

0 

1.25 

1.00 

0 

25.00 

2.00 

15.00 

300.0 

250.0 

2.75 

2.75 

0 

1.25 

1.00 

0 

25.00 

2.00 

200.00 

300.0 

100.0 

2.75 

2.75 

0 

1.25 

1.00 

0 

25.00 

2.00 

200.00 

400.0 

100.0 

2.75 

2.75 

0 

1.25 

1.00 

0 

25.00 

2.00 

200.00 

400.0 

250.0 

2.75 

2.75 

0 

1.25 

1.00 

0 

25.00 

2.00 

200.00] 


track=Track(:,1:2) ; 
SurfaceTime = Track(:,9); 
SurfPhase = Track(:,8); 


% Read in waypoints from track data assumes track is loaded 
for j=l:No_tracks, 

X_Way_c(j) = track(j,l); 

Y_Way_c(j) = track(j,2); 

end; 


PrevX_Way_c(1) = X(1); 
PrevY_Way_c(1) = Y(1); 


% Set rudder angle saturation: 
sat = 9; % Degrees 


% Set Watch Radius: 

W_R = 2.0; 

% Set dead-reckoning/look-ahead distance: 
rabbit = 7; 


x(:,l) = [v(1) ;r(1) ;psi (1) ] ; 
xf(:,l) = [vf(1);rf(1);psif(1)]; 

Eta_FlightHeading =0.5; % Lowered this from 1.0 on ARIES model 

Phi_FlightHeading =0.1; % Lowered this from 0.5 on ARIES model 

% Below for tanh 
Eta_CTE = 0.05; 

Eta_CTE_Min = 1.0; 

Phi_CTE = 0.2; 

UC = 0.0; 

Vc = 0.0; 

SegLen(l) = sqrt((X_Way_c(1)-PrevX_Way_c(1))^2+(Y_Way_c(1)- 
PrevY_Way_c(1))^2); 

psi_track(l) = atan2(Y_Way_c(1)-PrevY_Way_c(1),X_Way_c(1)- 
PrevX_Way_c(1)); 
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for j=2:No_tracks, 

SegLen(j) = sqrt((X_Way_c(j)-X_Way_c(j-1))^2+(Y_Way_c(j)-Y_Way_c(j- 
1 ))" 2 ) ; 

psi_track(j) = atan2(Y_Way_c(j)-Y_Way_c(j-1),X_Way_c(j)-X_Way_c(j- 
1 ) ) ; 
end; 


j=l; 

Sigma = [] ; 

Depth_com = [] ; 

dr= [] ; 
drl = [] ; 
drl(l) = 0.0; 
psint= [] ; 

Depth_com(1) = 5.0; 

WayPointVertDist_com = [5.0 5.0 5.0 5.0 5.0 5.0 5.0]; 

% Initialize path planning 

start=zeros(1,length(t)-1);offset=start; 

Xdev=startYdev=start ; 
count=0; 
inplan=0; 
psiapf=start; 

% Begin mission simulation 
for i=l:length(t)-1 

Depth_com(i) = WayPointVertDist_com(j); 

X_Way_Error(i) = X_Way_c(j) - X(i); 

Y_Way_Error(i) = Y_Way_c(j) - Y(i); 

% DeWrap psi to within +/- 2.0*pi; 
psi_cont(i) = psi(i); 

while (abs(psi_cont(i)) > 2.0*pi) 

psi_cont(i) = psi_cont(i) - sign(psi_cont(i))*2.0*pi; 
end; 


% DeWrap psi_error to within +/- pi; 
psi_errorCTE(i) = psi_cont(i) - psi_track(j); 

while (abs(psi_errorCTE(i)) > pi) 

psi_errorCTE(i) = psi_errorCTE(i) - sign(psi_errorCTE(i))*2.0*pi 

end; 


% **Always calculate this** 

Beta = 0.0; 

cpsi_e = cos(psi_errorCTE(i)+Beta); 
spsi_e = sin(psi_errorCTE(i)+Beta); 
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s(i) = [X_Way_Error(i),Y_Way_Error(i)]* ... 

[(X_Way_c(j)-PrevX_Way_c(j)) , (Y_Way_c(j)-PrevY_Way_c(j))] ' 

% s is distance to go projected to track line(goes from 0-100%L) 
s(i) = s(i)/SegLen(j); 

Ratio=(1.0-s(i)/SegLen(j))*100.0 ; 

% ss is the radial distance to go to next WP 
ss(i) = sqrt(X_Way_Error(i)^2 + Y_Way_Error(i)^2); 

% dp is the angle between line of sight and current track line 
dp (i) = ... 

atan2( (Y_Way_c(j)-PrevY_Way_c(j)),(X_Way_c(j)- 
PrevX_Way_c(j)) ) ... 

- atan2( Y_Way_Error(i),X_Way_Error(i) ); 

if (dp(i) > pi), 

dp(i) = dp(i) - 2.0*pi; 
end; 


cte(i) = s(i)*sin(dp(i)); 

if ( abs(psi_errorCTE(i)) >= 00.0*pi/180.0) 

% Use LOS Control 

LOS(i) = 1; 

psi_comLOS(i) = atan2(Y_Way_Error(i),X_Way_Error(i)); 
cc=0 ; 

psioalook(i)=0; 
r_com(i)=0.0; 

if coO 

psioatot(i)=psioalook(i)/cc ; 

else 

psioatot(i)=psioalook(i); 

end; 

% Integral control - limit error only activate if i>2 
psint(1)=0; 
ctel(i)=cte (i) ; 

if (abs(cte(i))> 2), ctel(i) =2*sign(cte(i)) ;end; 

if (i>l), psint(i)=psint(i-1)+dt*cte(i) ;end; 

psi_errorLOS(i) = psi_comLOS(i) - psi_cont(i)- 
(atan2(cte(i),rabbit))*1.0 -0.0*psint(i)+ 

0.0*psioatot(i)+1.0*psiapf(i); 
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if (abs(psi_errorLOS(i)) > pi), 
psi_errorLOS(i) = ... 
psi_errorLOS(i) - 

2.0*pi*psi_errorLOS(i)/abs(psi_errorLOS(i)); 
end; 

Sigma_FlightHeading(i) = (-S(1,1)*v(i))*0.0+S(2,1)*(r_com(i) - 

r(i)) + S(3,1)*psi_errorLOS(i); 

% Have taken out v influence in Sigma_FlightHeading above 

dr(i) = (-k(1,1)*v(i))*0.0-k(1,2)*r (i) ... 

Eta_FlightHeading*tanh(Sigma_FlightHeading(i)/Phi_FlightHeading); 
else 

% Use GTE Controller 
LOS(i) = 0; 

if (cpsi_e ~= 0.0), % Trap Div. by Zero ! 

% SMC Soln 

Sigma(i) = Uo*rRM (i) *cpsi_e + Laml*Uo*spsi_e + Lam2*cte (i) ,- 

dr(i) = (1.0/(Uo*a*cpsi_e))*(-Uo*b*rRM(i)*cpsi_e + 

Uo*rRM(i)^2*spsi_e ... 

- Laml*Uo*rRM(i)*cpsi_e - Lam2*Uo*spsi_e - 
2.0*Eta_CTE*(Sigma(i)/Phi_CTE)); 

else 

dr(i) = dr(i-1); 
end; 

end;% End of CTE Controller 

if (abs(dr(i)) > sat*pi/180) % change from 0.4 radians on ARIES 

dr(i) = sat*pi/180*sign(dr(i)); end; 

%Range and Bearing 
Room =10; %in m 

range(i)=sqrt((Y(i)-Yf(i))^2+(X(i)-Xf(i))^2); 

R_err(i)=range(i)-Room; 

U(i)=(-sign(R_err(i))*range(i)+Uo*cos(psi(i))*(Xf(i)- 
X(i))+Uo*sin(psi(i))*(Yf(i)-Y(i)))/(cos(psi(i))*(Xf(i)- 
X(i))+sin(psi(i))*(Yf(i)-Y(i))); 

Boom = 0; 

bearing(i)=atan2((Y(i)-Yf(i)) , (X(i)-Xf(i)))-psif(i) ; 
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B_err(i)=bearing(i)-Bcom; 


if (abs(B_err(i))>pi), B_err(i)=B_err(i)-2*pi*sign(B_err(i)) ;end; 


drf(i)=-5*B_err(i)+10*rf(i); 
maxspeed = 2; 
minspeed = 1 

if U(i)<minspeed 
U(i)=minspeed; 

end; 

if U(i)>maxspeed 
U(i)=maxspeed; 

end; 


% State Variable Formulation 

x_dot(:,i+l) = [ A(l,l)*v(i) + A(l,2)*r(i) + B(l)*dr(i); 

A(2,l)*v(i) + A(2,2)*r(i) + B(2)*dr(i); 

r (i) ] ; 

x(:,i+l) = X(:,i)+dt*x_dot(:,i); 
v(i+l) = x(l,i+1); 
r(i + 1) = X(2,i + 1) ; 

psi(i+l) = x(3,i+l); 
rRM(i + 1) = r(i + 1) ; 

x_dotf(:,i+1) = [ A(l,l)*vf(i) + A(l,2)*rf(i) + B(l)*drf(i); 

A(2,l)*vf(i) + A(2,2)*rf(i) + B(2)*drf(i); 

rf (i)] ; 

xf ( : ,i + 1) = xf(: ,i)+dt*x_dotf(: ,i) ; 

vf(i+l) = xf(l,i+l); 

rf (i + 1) = xf(2,i + l); 

psif (i + 1) = xf(3,i + l); 

rRMf(i + 1) = rf (i + 1) ; 

% Kinematics 

X(i+1) = X(i) + (Uc + (Uo)*cos(psi(i)) - v(i)*sin(psi(i)) )*dt; 

Y(i+1) = Y(i) + (Vc + (Uo)*sin(psi(i)) + v(i)*cos(psi(i)) )*dt; 

Xf(i+1) = Xf(i) + (Uc + (U(i))*cos(psif(i)) - vf(i)*sin(psif(i)) )*dt; 

Yf(i+1) = Yf(i) + (Vc + (U(i))*sin(psif(i)) + vf(i)*cos(psif(i)) )*dt; 

% Check to see if we are within the WR or if we passed the WP 

% Change to next WP if radial distance to go is less than rabbit 
% distance or if we passed the WP or if we are within the WR 

if (sqrt(X_Way_Error(i)^2.0 + Y_Way_Error(i)^2.0) <= W_R | s(i) < 

0.0 I ss(i)<rabbit ) 

disp(sprintf( 'WayPoint %d Reached j)); 
if (j==No_tracks), 

% PLOT PART = 1; 
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break; 

end; 

PrevX_Way_c(j +1) = X_Way_c(j) ; 
PrevY_Way_c(j +1) = Y_Way_c(j) ; 
j=j+l; 
end; 


end; 


dr(i+1) = dr(i); 
cte(i + 1) = cte (i) ; 
s(i + 1) = s(i) ; 
ss(i + 1) = ss (i) ; 
range(i+1)=range(i); 
bearing(i+1)=bearing(i); 

R_err(i+1)=R_err(i); 

U(i+l) =U(i) ; 

figure (1) ; 

for i=l:100:length(Y) 

plot(Y(i),X(i),'r.'); title(' Multi-Vehicle Mission Run' ); xlabel( 'Y 
(m)' ), ylabel( 'X (m)' ); legend( 'Leader Vehicle' , 'Follower Vehicle' ); 
axis([0 300 -50 450]); 
hold on 

plot(Yf(i),Xf(i), 'g*' ); 
axis( [0 300 -50 450] ) ; 
grid on; hold on, 
end; 


figure(2); 

plot(range, 'r .') ; title('Range Between Leader and Follower Vehicle'); 
ylabel( 'Distance (m)' ); 
grid on; 

figure(3); 

plot(bearing.*180/pi ,' b .') ; title (' Bearing Between Leader and Follower 
Vehicle' ); ylabel( 'Bearing (deg)' ); 
grid on; 
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